import static org.lwjgl.opengl.GL11.GL_COLOR_BUFFER_BIT;
import static org.lwjgl.opengl.GL11.GL_LINE_LOOP;
import static org.lwjgl.opengl.GL11.GL_LINE_STRIP;
import static org.lwjgl.opengl.GL11.GL_MODELVIEW;
import static org.lwjgl.opengl.GL11.GL_MODELVIEW_MATRIX;
import static org.lwjgl.opengl.GL11.GL_PROJECTION;
import static org.lwjgl.opengl.GL11.glBegin;
import static org.lwjgl.opengl.GL11.glClear;
import static org.lwjgl.opengl.GL11.glColor3d;
import static org.lwjgl.opengl.GL11.glColor3f;
import static org.lwjgl.opengl.GL11.glEnd;
import static org.lwjgl.opengl.GL11.glFlush;
import static org.lwjgl.opengl.GL11.glFrustum;
import static org.lwjgl.opengl.GL11.glGetDouble;
import static org.lwjgl.opengl.GL11.glLoadIdentity;
import static org.lwjgl.opengl.GL11.glMatrixMode;
import static org.lwjgl.opengl.GL11.glRotatef;
import static org.lwjgl.opengl.GL11.glTranslated;
import static org.lwjgl.opengl.GL11.glVertex3d;
import static org.lwjgl.opengl.GL11.glVertex3f;
import static org.lwjgl.opengl.GL11.glViewport;

import java.nio.ByteBuffer;
import java.nio.DoubleBuffer;
import java.util.ArrayList;
import java.util.Collection;
import java.util.concurrent.BlockingQueue;
import java.util.concurrent.LinkedBlockingQueue;

import org.lwjgl.input.Keyboard;
import org.lwjgl.input.Mouse;
import org.lwjgl.opengl.Display;
import org.lwjgl.opengl.DisplayMode;

public class GLWindow extends Thread {
	
	private final double steplength = 0.01;
	private boolean freeCamera = true;
	private double speedBoost = .5;

	private double rotateSpeed =1;
	private double movementSpeed = .5; 
	private boolean gridEnabled = false;
	private boolean MOVE = false;
	private double[] currentTrackLocation = { 0, 6, 3 };
	private double[] currentTrackDirection = { 0, 0 };
	private Cart theCart = new Cart(0, 6, 3, 5, currentTrackDirection.clone());
	private int realPathIterator = 0;
	private float shit, shack;
	
	private ByteBuffer shitState = ByteBuffer.allocateDirect(128);
	private DoubleBuffer lastState = shitState.asDoubleBuffer();
	private float n = 0;
	private double piover180 = Math.PI / 180;

	private double[] sineTable = buildSineTable();
	private double[] cosineTable = buildCosineTable();

	private double scalex, scaley, X, Y, Z;
	private float pitch, yaw, roll;

	private double[] zpos = { 0, 0, 1 };
	private double[] zneg = { 0, 0, -1 };
	private double[] xpos = { 1, 0, 0 };
	private double[] xneg = { -1, 0, 0 };

	
	private ArrayList<Double> theRealPathAccelerationVectors = new ArrayList<Double>();
	
	private ArrayList<double[]> theRealPath = buildPath();
	private ArrayList<double[]> theAddPath = new ArrayList<double[]>();
	
	public GLWindow() {
		
	}
	
	public void run() {
		try
		{

			Display.setDisplayModeAndFullscreen(new DisplayMode(1280, 1024));
			Display.create();

		} catch (Exception e)
		{
		}

		glMatrixMode(GL_PROJECTION);
		glLoadIdentity();
		glFrustum(-1.0, 1.0, -1.0, 1.0, 1.2, 2000.0);

		glMatrixMode(GL_MODELVIEW);
		glViewport(0, 0, 1280, 1024);

		while (!Display.isCloseRequested())
		{
			theRealPath.addAll(theAddPath);
			theAddPath.clear();
			try
			{
				display();
			} catch (Exception e)
			{
				e.printStackTrace();
				System.exit(1);
			}

			// You see this line?
			Display.update();
			// Don't you DARE remove it!
			// Doing so will lock up the entire machine!
		}
		Display.destroy();
	}
	
	public ArrayList<double[]> buildPath()
	{
		ArrayList<double[]> path = new ArrayList<double[]>();

		// path.addAll(buildStraightPiece(1, new double[] { -5, 0, 0 }, new
		// double[] { 0, 0, 1 }));

		path.addAll(buildAnyPiece(4, currentTrackLocation, 0, 0));

		return path;
	}
	
	public void display() throws InterruptedException
	{
		glMatrixMode(GL_MODELVIEW);
		glClear(GL_COLOR_BUFFER_BIT);
		glLoadIdentity();

		getInput();
		
		// drawCrosshair(white);

		glRotatef(pitch, 1, 0, 0);
		glRotatef(yaw, 0, 1, 0);
		glRotatef(roll, 0, 0, 1);

		glTranslated(-theCart.location[0], -(theCart.location[1] +1), -theCart.location[2]);

		// This is important otherwise your motion is too fast.
		Thread.sleep(10);

		drawCube(0, 0, -4, Colors.RED.getColor());
		drawCube(4, 0, 0, Colors.BLUE.getColor());
		drawCube(0, 0, 4, Colors.GREEN.getColor());
		drawCube(-4, 0, 0, Colors.YELLOW.getColor());

		drawCube(0, shit, 0, Colors.WHITE.getColor());
		
//		drawSineWave();
//		drawCosineWave();
		drawPath(theRealPath, Colors.GREEN.getColor());

		if (gridEnabled)
			drawGrid(300, -5, 300, Colors.YELLOW.getColor(), 7);

		if(getMOVE()){
			if(realPathIterator <0)
				realPathIterator = theRealPath.size()-1;
			theCart.location = theRealPath.get(realPathIterator);
			drawCubev(theCart.location, Colors.RED.getColor(), 1);
			

			realPathIterator += theCart.updateSpeed(realPathIterator, theRealPathAccelerationVectors);

			if (realPathIterator >= theRealPath.size())
				realPathIterator = 0;
		}

		glFlush();
	}
	
	public void getInput() {
		if (Mouse.isButtonDown(0))
		{
			X = (Mouse.getX() - Display.getDisplayMode().getWidth() / 2.0) / 100.0;
			Y = (Mouse.getY() - Display.getDisplayMode().getWidth() / 2.0) / 100.0;

		}
		// Does not handle roll!
		if (Keyboard.isKeyDown(Keyboard.KEY_UP))
		{

			Z += movementSpeed * Math.cos(piover180 * yaw);
			Y += movementSpeed * Math.sin(piover180 * pitch);
			X -= movementSpeed * Math.sin(piover180 * yaw);

		}
		// Does not handle roll!
		if (Keyboard.isKeyDown(Keyboard.KEY_DOWN))
		{
			Z -= movementSpeed * Math.cos(piover180 * yaw);
			Y -= movementSpeed * Math.sin(piover180 * pitch);
			X += movementSpeed * Math.sin(piover180 * yaw);

		}
		if (Keyboard.isKeyDown(Keyboard.KEY_RIGHT))
		{
			yaw+=rotateSpeed;
			// System.out.println(yaw+"y");
		}
		if (Keyboard.isKeyDown(Keyboard.KEY_LEFT))
		{
			yaw-=rotateSpeed;
			// System.out.println(yaw+"y");
		}
		if (Keyboard.isKeyDown(Keyboard.KEY_1))
		{
			pitch+=rotateSpeed;
			// System.out.println(pitch+"p");
		}
		if (Keyboard.isKeyDown(Keyboard.KEY_4))
		{
			pitch-=rotateSpeed;
			// System.out.println(pitch+"p");
		}
		if (Keyboard.isKeyDown(Keyboard.KEY_SPACE))
		{
			forward();
		}
		if (Keyboard.isKeyDown(Keyboard.KEY_W))
		{
			up();
		}
		if (Keyboard.isKeyDown(Keyboard.KEY_S))
		{
			down();
		}
		if (Keyboard.isKeyDown(Keyboard.KEY_A))
		{
			left();
		}
		if (Keyboard.isKeyDown(Keyboard.KEY_D))
		{
			right();
		}
		if (Keyboard.isKeyDown(Keyboard.KEY_G))
		{
			gridEnabled = !gridEnabled;
		}
		if(Keyboard.isKeyDown(Keyboard.KEY_M)){
			setMove(!getMOVE());
		}
		if (Keyboard.isKeyDown(Keyboard.KEY_ADD))
		{
			theCart.g -= 1;
			System.out.println("GRAVITY = -"+theCart.g);
		}
		if(Keyboard.isKeyDown(Keyboard.KEY_SUBTRACT))
		{
			theCart.g+=1;
			System.out.println("GRAVITY = "+theCart.g);
		}
		if(Keyboard.isKeyDown(Keyboard.KEY_I))
		{
			movementSpeed+=.1;
			System.out.println("MOVEMENT SPEED = "+movementSpeed);
		}
		if(Keyboard.isKeyDown(Keyboard.KEY_U))
		{
			movementSpeed-=.1;
			System.out.println("MOVEMENT SPEED = "+movementSpeed);
		}
		if(Keyboard.isKeyDown(Keyboard.KEY_C))
		{
			rotateSpeed+=.1;
			System.out.println("ROTATION SPEED = "+rotateSpeed);
		}
		if(Keyboard.isKeyDown(Keyboard.KEY_X))
		{
			rotateSpeed-=.1;
			System.out.println("ROTATION SPEED = "+rotateSpeed);
		}
		if(Keyboard.isKeyDown(Keyboard.KEY_B))
		{
			theCart.speed /=1.15;
		}
		if(Keyboard.isKeyDown(Keyboard.KEY_NUMPAD1))
		{
			theCart.speed += speedBoost;
		}
		if(Keyboard.isKeyDown(Keyboard.KEY_NUMPAD2))
		{
			theCart.speed -= speedBoost;
		}
		if(Keyboard.isKeyDown(Keyboard.KEY_NUMPAD7))
		{
			speedBoost += .01;
			System.out.println("SPEEDBOOST = "+ speedBoost);
		}
		if(Keyboard.isKeyDown(Keyboard.KEY_NUMPAD8))
		{
			speedBoost -= .01;
			System.out.println("SPEEDBOOST = "+speedBoost);
		}
		if(Keyboard.isKeyDown(Keyboard.KEY_F))
		{
			freeCamera = !freeCamera;
		}
	}
	
	public void drawPath(ArrayList<double[]> path, float[] color)
	{
		glColor3f(color[0], color[1], color[2]);
		glBegin(GL_LINE_STRIP);
		for (double[] p : path)
		{
			glVertex3d(p[0], p[1], p[2]);
		}
		glEnd();
	}
	
	public ArrayList<double[]> buildAnyPiece(double l, double[] loc, double pitch, double yaw)
	{
		
		double[] curDir = currentTrackDirection;
		double[] curLoc = loc;
		ArrayList<double[]> piece = new ArrayList<double[]>();

		int steps = (int) (100 * l);
		double angular_pitch_step = pitch / steps;
		double angular_yaw_step = yaw / steps;

		int i;
		for (i = 0; i < steps; i++)
		{	
			piece.add(curLoc.clone());

			curDir[0] += angular_pitch_step; // THETA
			curDir[1] += angular_yaw_step; // PHI

			Double deltay = getStepLength() * sine(curDir[0]);
			theRealPathAccelerationVectors.add(deltay);
			
			curLoc[0] += getStepLength() * cosine(curDir[0]) * sine(curDir[1]);
			curLoc[1] += deltay;
			curLoc[2] += getStepLength() * cosine(curDir[0]) * cosine(curDir[1]);
		}

		currentTrackLocation = curLoc;
		currentTrackDirection = curDir;

		return piece;
	}	
	
	public void forward()
	{
		theAddPath.addAll(buildAnyPiece(1, currentTrackLocation, 0, 0));
	}

	public void left()
	{
		theAddPath.addAll(buildAnyPiece(1, currentTrackLocation, 0, 3));
	}

	public void right()
	{
		theAddPath.addAll(buildAnyPiece(1, currentTrackLocation, 0, -3));
	}

	public void up()
	{
		theAddPath.addAll(buildAnyPiece(1, currentTrackLocation, 1, 0));
	}

	public void down()
	{
		theAddPath.addAll(buildAnyPiece(1, currentTrackLocation, -1, 0));
	}
	
	public void delete() {
		if(theAddPath.size() != 0) {
			System.out.println("delete in if");
			theAddPath.remove(theAddPath.size() - 1);
		} else { 
			System.out.println("delete in else");
			theRealPath.remove(theRealPath.size() - 1);
		}
	}
	
	public void pollInput()
	{

		if (Mouse.isButtonDown(0))
		{
			int x = Mouse.getX();
			int y = Mouse.getY();

			System.out.println("MOUSE DOWN @ X: " + x + " Y: " + y);
		}

		if (Keyboard.isKeyDown(Keyboard.KEY_SPACE))
		{
			System.out.println("SPACE KEY IS DOWN");
		}

		while (Keyboard.next())
		{
			if (Keyboard.getEventKeyState())
			{
				if (Keyboard.getEventKey() == Keyboard.KEY_A)
				{
					System.out.println("A Key Pressed");
				}
				if (Keyboard.getEventKey() == Keyboard.KEY_S)
				{
					System.out.println("S Key Pressed");
				}
				if (Keyboard.getEventKey() == Keyboard.KEY_D)
				{
					System.out.println("D Key Pressed");
				}
			} else
			{
				if (Keyboard.getEventKey() == Keyboard.KEY_A)
				{
					System.out.println("A Key Released");
				}
				if (Keyboard.getEventKey() == Keyboard.KEY_S)
				{
					System.out.println("S Key Released");
				}
				if (Keyboard.getEventKey() == Keyboard.KEY_D)
				{
					System.out.println("D Key Released");
				}
			}
		}
	}
	
	public void drawGrid(int x, int y, int z, float[] color, double resolution)
	{
		glColor3f(color[0], color[1], color[2]);
		for (double i = x * -1; i < x; i += resolution)
		{
			glBegin(GL_LINE_STRIP);
			glVertex3d(i, y, -z);
			glVertex3d(i, y, z);
			glEnd();
		}
		for (double i = z * -1; i < z; i += resolution)
		{
			glBegin(GL_LINE_STRIP);
			glVertex3d(-x, y, i);
			glVertex3d(x, y, i);
			glEnd();
		}
	}
	
	
	public void drawCube(float x, float y, float z, float[] color)
	{
		glColor3f(color[0], color[1], color[2]);
		glBegin(GL_LINE_STRIP);
		glVertex3f(0 + x, 0 + y, 0 + z);
		glVertex3f(1 + x, 0 + y, 0 + z);
		glVertex3f(1 + x, 1 + y, 0 + z);
		glVertex3f(0 + x, 1 + y, 0 + z);

		glVertex3f(0 + x, 1 + y, -1 + z);
		glVertex3f(0 + x, 0 + y, -1 + z);
		glVertex3f(1 + x, 0 + y, -1 + z);
		glVertex3f(1 + x, 1 + y, -1 + z);
		glVertex3f(0 + x, 1 + y, -1 + z);

		glVertex3f(0 + x, 1 + y, 0 + z);
		glVertex3f(0 + x, 0 + y, 0 + z);
		glVertex3f(0 + x, 0 + y, -1 + z);
		glVertex3f(1 + x, 0 + y, -1 + z);
		glVertex3f(1 + x, 0 + y, 0 + z);
		glVertex3f(1 + x, 1 + y, 0 + z);
		glVertex3f(1 + x, 1 + y, -1 + z);
		glEnd();

	}
	
	public void drawCubed(double x, double y, double z, float[] color)
	{
		glColor3f(color[0], color[1], color[2]);
		glBegin(GL_LINE_STRIP);
		glVertex3d(0 + x, 0 + y, 0 + z);
		glVertex3d(1 + x, 0 + y, 0 + z);
		glVertex3d(1 + x, 1 + y, 0 + z);
		glVertex3d(0 + x, 1 + y, 0 + z);

		glVertex3d(0 + x, 1 + y, -1 + z);
		glVertex3d(0 + x, 0 + y, -1 + z);
		glVertex3d(1 + x, 0 + y, -1 + z);
		glVertex3d(1 + x, 1 + y, -1 + z);
		glVertex3d(0 + x, 1 + y, -1 + z);

		glVertex3d(0 + x, 1 + y, 0 + z);
		glVertex3d(0 + x, 0 + y, 0 + z);
		glVertex3d(0 + x, 0 + y, -1 + z);
		glVertex3d(1 + x, 0 + y, -1 + z);
		glVertex3d(1 + x, 0 + y, 0 + z);
		glVertex3d(1 + x, 1 + y, 0 + z);
		glVertex3d(1 + x, 1 + y, -1 + z);
		glEnd();
	}
	
	static void drawCubev(double[] xyz, float[] color, double scale)
	{
		glColor3f(color[0], color[1], color[2]);
		glBegin(GL_LINE_STRIP);
		glVertex3d(0 + xyz[0], 0 + xyz[1], 0 + xyz[2]);
		glVertex3d(scale + xyz[0], 0 + xyz[1], 0 + xyz[2]);
		glVertex3d(scale + xyz[0], scale + xyz[1], 0 + xyz[2]);
		glVertex3d(0 + xyz[0], scale + xyz[1], 0 + xyz[2]);

		glVertex3d(0 + xyz[0], scale + xyz[1], -scale + xyz[2]);
		glVertex3d(0 + xyz[0], 0 + xyz[1], -scale + xyz[2]);
		glVertex3d(scale + xyz[0], 0 + xyz[1], -scale + xyz[2]);
		glVertex3d(scale + xyz[0], scale + xyz[1], -scale + xyz[2]);
		glVertex3d(0 + xyz[0], scale + xyz[1], -scale + xyz[2]);

		glVertex3d(0 + xyz[0], scale + xyz[1], 0 + xyz[2]);
		glVertex3d(0 + xyz[0], 0 + xyz[1], 0 + xyz[2]);
		glVertex3d(0 + xyz[0], 0 + xyz[1], -scale + xyz[2]);
		glVertex3d(scale + xyz[0], 0 + xyz[1], -scale + xyz[2]);
		glVertex3d(scale + xyz[0], 0 + xyz[1], 0 + xyz[2]);
		glVertex3d(scale + xyz[0], scale + xyz[1], 0 + xyz[2]);
		glVertex3d(scale + xyz[0], scale + xyz[1], -scale + xyz[2]);
		glEnd();

	}
	public double sine(double angle)
	{
		if (angle < 0)
		{
			return sineTable[(int) (((angle * -1) % 360) * 100)] * -1;
		} else
		{
			return sineTable[(int) ((angle % 360) * 100)];
		}
	}

	public double cosine(double angle)
	{
		return cosineTable[Math.abs((int) ((angle % 360) * 100))];
	}
	
	public  double[] buildSineTable()
	{
		double piover180 = (Math.PI / 180.0);
		double[] Table = new double[36001];
		int intdegree = 0;
		for (double degree = 0; degree < 360; degree += 0.01)
		{

			Table[intdegree] = Math.sin(piover180 * degree);

			intdegree++;
		}
		return Table;
	}

	public  double[] buildCosineTable()
	{
		double piover180 = (Math.PI / 180.0);
		double[] cosineTable = new double[36001];
		int intdegree = 0;
		for (double degree = 0; degree < 360; degree += 0.01)
		{

			cosineTable[intdegree] = Math.cos(piover180 * degree);
			intdegree++;

		}
		return cosineTable;
	}
	
	
	public double getStepLength() {
		return steplength;
	}

	public void setMove(boolean move) {
		MOVE = move;
	}

	public boolean getMOVE() {
		return MOVE;
	}
	
	public void toggleGrid() {
		gridEnabled = !gridEnabled;
	}



}
//	public float FPS;
//	private int numFrames;
//	private float fpsUpdateInterval;
//	private float lastUpdate;
//	
//	private static final double DEFAULTLENGTH = 5;
//	private static double[] location =  { -5, 0, 0 };
//	private static double[] directionVector = { 0, 0, 1 };
//	private static ArrayList<double[]> theRealPath = new ArrayList<double[]>();
//	private BlockingQueue<double[]> pathQueue = new LinkedBlockingQueue<double[]>();
//	static int realPathIterator = 0;
//	private boolean coasterIsRunning;
//	
//	static double scalex, scaley, X, Y, Z;
//	static float pitch, yaw, roll;
//	static double piover180 = Math.PI / 180;
//	
//	static double[] sineTable = buildSineTable();
//	static double[] cosineTable = buildCosineTable();
//	
//	static float[] red = { 1, 0, 0 };
//	static float[] blue = { 0, 0, 1 };
//	static float[] green = { 0, 1, 0 };
//	static float[] yellow = { 1, 1, 0 };
//	static float[] white = { 1, 1, 1 };
//	
//	static double[] zpos = { 0, 0, 1 };
//	static double[] zneg = { 0, 0, -1 };
//	static double[] xpos = { 1, 0, 0 };
//	static double[] xneg = { -1, 0, 0 };;
//	
//	public GLWindow() {
//		FPS = 0;
//		numFrames = 0;
//		fpsUpdateInterval = (float) 0.5;
//		lastUpdate = System.nanoTime();
//	}
//	
//	public void updateFPS() {
//		numFrames += 1;
//		float currentUpdate = System.nanoTime();
//		
//		if( currentUpdate - lastUpdate > fpsUpdateInterval )
//		  {
//		    FPS = numFrames / (currentUpdate - lastUpdate);
//		    lastUpdate = currentUpdate;
//		    numFrames = 0;
//		  }
//	}
//	
//	static ArrayList<double[]> buildPath()
//	{
//		ArrayList<double[]> path = new ArrayList<double[]>();
//
//		path.addAll(buildStraightPiece(1, new double[] { -5, 0, 0 }, new double[] { 0, 0, 1 }));
//		path.addAll(buildStraightPiece(2, new double[] { -5, 0, 1 }, new double[] { 0, 1, 1 }));
//		path.addAll(buildStraightPiece(1, new double[] { -5, 2, 3 }, new double[] { 0, -1, 1 }));
//		path.addAll(buildStraightPiece(1, new double[] { -5, 1, 4 }, new double[] { 1, 0, 0 }));
//		path.addAll(buildStraightPiece(1, new double[] { -4, 1, 4 }, new double[] { 0, 0, 1 }));
//		path.addAll(buildStraightPiece(1, new double[] { -4, 1, 5 }, new double[] { 1, 0, 0 }));
//
//		return path;
//	}
//	
//	static ArrayList<double[]> buildStraightPiece(double length, double[] location, double[] directionVector)
//	{
//		ArrayList<double[]> piece = new ArrayList<double[]>();
//		int steps = (int) length * 100;
//
//		for (int i = 0; i < steps; i++)
//		{
//			double[] point = new double[] { i * .01 * directionVector[0] + location[0], i * .01 * directionVector[1] + location[1], i * .01 * directionVector[2] + location[2] };
//			piece.add(point);
//			updateLocation(length);
//		}
//
//		return piece;
//	}
//	
//	public static void updateLocation(double length) {
//		int i = 0;
//		double[] dVector = new double[3];
//		for(double num : directionVector) {
//			dVector[i] = num * length;
//			i++;
//		}
//		i = 0;
//		for(double num : location) {
//			location[i] = num + dVector[i];
//			i++;
//		}
//	}
//	
//	public static void updateDirectionVector() {
//		//TODO
//	}
//	
//	
//	public void run() {
//		try
//		{
//			Display.setDisplayModeAndFullscreen(new DisplayMode(1280, 1024));
//			Display.create();
//
//		} catch (Exception e)
//		{
//		}
//
//		glMatrixMode(GL_PROJECTION);
//		glLoadIdentity();
//		glFrustum(-1.0, 1.0, -1.0, 1.0, 1.5, 200.0);
//
//		glMatrixMode(GL_MODELVIEW);
//		glViewport(0, 0, 1280, 1024);
//		
//		while (!Display.isCloseRequested())
//		{
////			updateFPS();
//			
//			theRealPath.addAll(pathQueue);
//			ArrayList<double[]> c = new ArrayList<double[]>();
//			pathQueue.removeAll(c);
//			try
//			{
////				ArrayList<double[]> yeah = new ArrayList<double[]>();
////				pathQueue.removeAll(yeah);
//				
//				display();
//			} catch (Exception e)
//			{
//				e.printStackTrace();
//				System.exit(1);
//			}
//
//			// You see this line?
//			Display.update();
//			// Don't you DARE remove it!
//			// Doing so will lock up the entire machine!
//
//		}
//		Display.destroy();
//	}
//	
//	static void drawPath(float[] color)
//	{
//		glColor3f(color[0], color[1], color[2]);
//		glBegin(GL_LINE_STRIP);
//		for (double[] p : theRealPath)
//		{
//			glVertex3d(p[0], p[1], p[2]);
//		}
//		glEnd();
//	}
//	
//	public void display() {
//		glMatrixMode(GL_MODELVIEW);
//		glClear(GL_COLOR_BUFFER_BIT);
//		glLoadIdentity();
//
////		drawCrosshair(white);
//		
//		if (Mouse.isButtonDown(0))
//		{
//			X = (Mouse.getX() - Display.getDisplayMode().getWidth() / 2.0) / 100.0;
//			Y = (Mouse.getY() - Display.getDisplayMode().getWidth() / 2.0) / 100.0;
//
//		}
//		if (Mouse.isButtonDown(2))
//		{
//			scalex = Mouse.getX() / 200.0;
//			scaley = Mouse.getY() / 200.0;
//		}
//
//		// Does not handle roll!
//		if (Keyboard.isKeyDown(Keyboard.KEY_UP))
//		{
//
//			Z += .5 * Math.cos(piover180 * yaw);
//			Y += .5 * Math.sin(piover180 * pitch);
//			X -= .5 * Math.sin(piover180 * yaw);
//
//		}
//		// Does not handle roll!
//		if (Keyboard.isKeyDown(Keyboard.KEY_DOWN))
//		{
//			Z -= .5 * Math.cos(piover180 * yaw);
//			Y -= .5 * Math.sin(piover180 * pitch);
//			X += .5 * Math.sin(piover180 * yaw);
//
//		}
//
//
//		if (Keyboard.isKeyDown(Keyboard.KEY_RIGHT))
//		{
//			yaw++;
//			// System.out.println(yaw+"y");
//		}
//		if (Keyboard.isKeyDown(Keyboard.KEY_LEFT))
//		{
//			yaw--;
//			// System.out.println(yaw+"y");
//		}
//		if (Keyboard.isKeyDown(Keyboard.KEY_1))
//		{
//			pitch++;
//			// System.out.println(pitch+"p");
//		}
//		if (Keyboard.isKeyDown(Keyboard.KEY_4))
//		{
//			pitch--;
//			// System.out.println(pitch+"p");
//		}
//		if (Keyboard.isKeyDown(Keyboard.KEY_X))
//		{
//			roll += 1;
//		}
//		if (Keyboard.isKeyDown(Keyboard.KEY_Z))
//		{
//			roll -= 1;
//		}
//
//		// drawCrosshair(white);
//
//		glRotatef(pitch, 1, 0, 0);
//		glRotatef(yaw, 0, 1, 0);
//		glRotatef(roll, 0, 0, 1);
//
//		
//		glTranslated(X, Y, Z);
//		
//		// This is important otherwise your motion is too fast.
//		try {
//			Thread.sleep(10);
//		} catch (InterruptedException e) {
//			// TODO Auto-generated catch block
//			e.printStackTrace();
//		}
//		
//		drawCube(0, 0, -4, red);
//		drawCube(4, 0, 0, blue);
//		drawCube(0, 0, 4, green);
//		drawCube(-4, 0, 0, yellow);
//		
//		drawGrid(300,-5,300, yellow, 4);
//		drawPath(green);
//		
//		if(coasterIsRunning) {
//			drawCubev(theRealPath.get(realPathIterator), new double[] {
//				cosine(realPathIterator / 100.0), cosine(realPathIterator / 100.0), 1
//			}, 2);
//		
//			realPathIterator += 4;
//			if (realPathIterator >= theRealPath.size())
//				realPathIterator = 0;
//		}
//		glFlush();
//	}
//
//	static void drawGrid(int x, int y, int z, float[] color, double resolution)
//	{
//		glColor3f(color[0], color[1], color[2]);
//		for (double i = x * -1; i < x; i+=resolution)
//		{
//			glBegin(GL_LINE_STRIP);
//			glVertex3d(i,y,-z);
//			glVertex3d(i,y,z);
//			glEnd();
//		}
//		for (double i = z * -1; i < z; i+=resolution)
//		{
//			glBegin(GL_LINE_STRIP);
//			glVertex3d(-x,y,i);
//			glVertex3d(x,y,i);
//			glEnd();
//		}
//	}
//	
//	static void drawCubev(double[] xyz, double[] color, double scale)
//	{
//		glColor3d(color[0], color[1], color[2]);
//		glBegin(GL_LINE_STRIP);
//		glVertex3d(0 + xyz[0], 0 + xyz[1], 0 + xyz[2]);
//		glVertex3d(scale + xyz[0], 0 + xyz[1], 0 + xyz[2]);
//		glVertex3d(scale + xyz[0], scale + xyz[1], 0 + xyz[2]);
//		glVertex3d(0 + xyz[0], scale + xyz[1], 0 + xyz[2]);
//
//		glVertex3d(0 + xyz[0], scale + xyz[1], -scale + xyz[2]);
//		glVertex3d(0 + xyz[0], 0 + xyz[1], -scale + xyz[2]);
//		glVertex3d(scale + xyz[0], 0 + xyz[1], -scale + xyz[2]);
//		glVertex3d(scale + xyz[0], scale + xyz[1], -scale + xyz[2]);
//		glVertex3d(0 + xyz[0], scale + xyz[1], -scale + xyz[2]);
//
//		glVertex3d(0 + xyz[0], scale + xyz[1], 0 + xyz[2]);
//		glVertex3d(0 + xyz[0], 0 + xyz[1], 0 + xyz[2]);
//		glVertex3d(0 + xyz[0], 0 + xyz[1], -scale + xyz[2]);
//		glVertex3d(scale + xyz[0], 0 + xyz[1], -scale + xyz[2]);
//		glVertex3d(scale + xyz[0], 0 + xyz[1], 0 + xyz[2]);
//		glVertex3d(scale + xyz[0], scale + xyz[1], 0 + xyz[2]);
//		glVertex3d(scale + xyz[0], scale + xyz[1], -scale + xyz[2]);
//		glEnd();
//
//	}
//	
//	static void drawCube(float x, float y, float z, float[] color)
//	{
//		glColor3f(color[0], color[1], color[2]);
//		glBegin(GL_LINE_STRIP);
//		glVertex3f(0 + x, 0 + y, 0 + z);
//		glVertex3f(1 + x, 0 + y, 0 + z);
//		glVertex3f(1 + x, 1 + y, 0 + z);
//		glVertex3f(0 + x, 1 + y, 0 + z);
//
//		glVertex3f(0 + x, 1 + y, -1 + z);
//		glVertex3f(0 + x, 0 + y, -1 + z);
//		glVertex3f(1 + x, 0 + y, -1 + z);
//		glVertex3f(1 + x, 1 + y, -1 + z);
//		glVertex3f(0 + x, 1 + y, -1 + z);
//
//		glVertex3f(0 + x, 1 + y, 0 + z);
//		glVertex3f(0 + x, 0 + y, 0 + z);
//		glVertex3f(0 + x, 0 + y, -1 + z);
//		glVertex3f(1 + x, 0 + y, -1 + z);
//		glVertex3f(1 + x, 0 + y, 0 + z);
//		glVertex3f(1 + x, 1 + y, 0 + z);
//		glVertex3f(1 + x, 1 + y, -1 + z);
//		glEnd();
//
//	}
//	
//
//	@Override
//	public void openWindow() {
//		this.start();
//	}
//
//	@Override
//	public void renderCart(Cart cart) {
//		// TODO Auto-generated method stub
//		
//	}
//
//	@Override
//	public void renderCoaster(Rollercoaster coaster) {
//		// TODO Auto-generated method stub
//		
//	}
//
//	@Override
//	public void renderPart(Part part) {
//		// TODO Auto-generated method stub
//		
//	}
//
//	@Override
//	public void renderRightCurve() {
//		// TODO Auto-generated method stub
//		
//	}
//	
//	public void renderDefaultStraightPiece() {
//		renderStraightPiece(DEFAULTLENGTH);
//	}
//	
//	public void renderStraightPiece(double length) {
//		ArrayList<double[]> piece = new ArrayList<double[]>();
//		int steps = (int) length * 100;
//
//		for (int i = 0; i < steps; i++)
//		{
//			double[] point = new double[] { i * .01 * directionVector[0] + location[0], i * .01 * directionVector[1] + location[1], i * .01 * directionVector[2] + location[2] };
//			try {
//				pathQueue.put(point);
//			} catch (InterruptedException e) {
//				// TODO Auto-generated catch block
//				e.printStackTrace();
//			}
//			piece.add(point);
//		}
//		
////		theRealPath.addAll(piece);
//		updateLocation(length);
//	}
//
//	@Override
//	public void renderUpCurve() {
//		// TODO Auto-generated method stub
//		
//	}
//
//	@Override
//	public void renderDownCurve() {
//		// TODO Auto-generated method stub
//		
//	}
//
//	@Override
//	public void renderLeftCurve() {
//		// TODO Auto-generated method stub
//		
//	}
//	
//
//	@Override
//	public void runCoaster() {
//		coasterIsRunning = true;
//	}
//	
//	public void stopCoaster() {
//		coasterIsRunning = false;
//	}
//
//	static double sine(double angle)
//	{
//		return sineTable[(int) ((angle % 360) * 100)];
//	}
//
//	static double cosine(double angle)
//	{
//		return cosineTable[(int) ((angle % 360) * 100)];
//	}
//	
//	public static double[] buildSineTable()
//	{
//		double piover180 = (Math.PI / 180.0);
//		double[] Table = new double[36001];
//		int intdegree = 0;
//		for (double degree = 0; degree < 360; degree += 0.01)
//		{
//
//			Table[intdegree] = Math.sin(piover180 * degree);
//
//			intdegree++;
//		}
//		return Table;
//	}
//
//	public static double[] buildCosineTable()
//	{
//		double piover180 = (Math.PI / 180.0);
//		double[] cosineTable = new double[36001];
//		int intdegree = 0;
//		for (double degree = 0; degree < 360; degree += 0.01)
//		{
//
//			cosineTable[intdegree] = Math.cos(piover180 * degree);
//			intdegree++;
//
//		}
//		return cosineTable;
//	}
//	
//}

